#include<Wire.h>
const int MPU = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
int breaking = A1;
int doublesignal = A0;

const int trigPin = 3;
const int echoPin = 4;
int green = 5;
int yellow = 8;
int red = 12;
long duration;
int distance;



void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  pinMode(breaking, OUTPUT);
  pinMode(doublesignal, OUTPUT);


  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT); 
  pinMode(red, OUTPUT);
  pinMode(yellow, OUTPUT);
  pinMode(green, OUTPUT);
  Serial.begin(9600);
}
void loop() {

  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 12, true);
  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();
  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  GyZ = Wire.read() << 8 | Wire.read();

  Serial.print("Accelerometer: ");
  Serial.print("X = "); Serial.print(AcX);
  Serial.print(" | Y = "); Serial.print(AcY);
  Serial.print(" | Z = "); Serial.println(AcZ);

  Serial.print("Gyroscope: ");
  Serial.print("X = "); Serial.print(GyX);
  Serial.print(" | Y = "); Serial.print(GyY);
  Serial.print(" | Z = "); Serial.println(GyZ);
  Serial.println(" ");
  delay(333);


  digitalWrite(doublesignal, LOW);
  digitalWrite(breaking, LOW);

  if (AcY < -4800 && AcY >= -8200 )
  {
    digitalWrite(breaking, 70);
    delay(700);
  }
  else if (AcY < -8200 && AcY >= -12000)
  {
    digitalWrite(breaking, 255);
    delay(700);
  }
  else if (AcY < -12000)
  {
    digitalWrite(breaking, HIGH);
    digitalWrite(doublesignal, HIGH);
    delay(500);
    digitalWrite(doublesignal, LOW);
    delay(500);
    digitalWrite(doublesignal, HIGH);
    delay(500);
    digitalWrite(doublesignal, LOW);
    delay(500);
    digitalWrite(doublesignal, HIGH);
  }
  else {
    digitalWrite(doublesignal, LOW);
    digitalWrite(breaking, LOW);
  }



  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;
  Serial.print("Distance: ");
  Serial.println(distance);

  digitalWrite(green, LOW);
  digitalWrite(yellow, LOW);
  digitalWrite(red, LOW);

  if (distance > 0 && distance <= 10) {

    digitalWrite(red, HIGH);
    digitalWrite(yellow, LOW);
    digitalWrite(green, LOW);


  }
  else if (distance > 10 && distance <= 20) {
    digitalWrite(yellow, HIGH);
    digitalWrite(red, LOW);
    digitalWrite(green, LOW);


  }
  else if (distance > 20 && distance <= 30) {
    digitalWrite(green, HIGH);
    digitalWrite(yellow, LOW);
    digitalWrite(red, LOW);

  }
  else {

    digitalWrite(green, LOW);
    digitalWrite(yellow, LOW);
    digitalWrite(red, LOW);
  }

}